#include <Arduino.h>
#include <servo1.h>

//计算角度0-180度

int calculatePWM(double degree)
{
    //20ms周期，高电平0.5-2.5ms，对应0-180度角度
    const float deadZone = 6.4; //对应0.5ms（0.5ms/(20ms/256）)
    const float max = 32;       //对应2.5ms
    if (degree < 0)
        degree = 0;
    if (degree > 180)
        degree = 180;
    return (int)(max - ((max - deadZone) / 180) * degree);
}

//舵机初始化
void servo_init()
{
    pinMode(servo1, OUTPUT);
    pinMode(servo2, OUTPUT);
    pinMode(guang, OUTPUT);
    pinMode(bian, INPUT | PULLDOWN);

    ledcSetup(channel, freq, resolution); // 设置通道
    ledcAttachPin(servo1, channel);       // 将通道与对应的引脚连接
    ledcSetup(channe2, freq, resolution); // 设置通道
    ledcAttachPin(servo2, channe2);       // 将通道与对应的引脚连接
}
//舵机打角函数
void servopull(double x, double y)
{
    double angle1, angle2;
    if (x > 0) //0-90度
    {
        angle1 = atan(y / x);
        angle2 = 90;
    }
    if (x < 0) //90-180度
    {
        angle1 = 180 - atan(y / (-x));
        angle2 = 90;
    }
    ledcWrite(channel, calculatePWM(angle1)); // 横向舵机输出PWM
    ledcWrite(channe2, calculatePWM(angle2)); // 纵向舵机输出PWM
    unbiu();
}
void biu()
{
    digitalWrite(guang, HIGH);
    delay(1000);
}
void unbiu()
{
    digitalWrite(guang, LOW);
    delay(1000);
}
void function()
{
    if (x >= -150)
    {
        x -= 20;
        servopull(x, 250);
        delay(1000);
        biu();
    }
} /*
void setup()
{
    Serial.begin(115200);
    servo_init();
    attachInterrupt(0, function, CHANGE);//当int.0电平改变时,触发中断函数blink
}

void loop()
{
    x = 150, y = 250;
    servopull(150, 250);
    delay(1000);
    biu();
 
}*/